/**************************************************************************\
 *
 *  This file is part of the Coin 3D visualization library.
 *  Copyright (C) by Kongsberg Oil & Gas Technologies.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  ("GPL") version 2 as published by the Free Software Foundation.
 *  See the file LICENSE.GPL at the root directory of this source
 *  distribution for additional information about the GNU GPL.
 *
 *  For using Coin with software that can not be combined with the GNU
 *  GPL, and for taking advantage of the additional benefits of our
 *  support services, please contact Kongsberg Oil & Gas Technologies
 *  about acquiring a Coin Professional Edition License.
 *
 *  See http://www.coin3d.org/ for more information.
 *
 *  Kongsberg Oil & Gas Technologies, Bygdoy Alle 5, 0257 Oslo, NORWAY.
 *  http://www.sim.no/  sales@sim.no  coin-support@coin3d.org
 *
\**************************************************************************/



#include <Inventor/engines/SoJumpVec3f.h>

#include "coindefs.h"
#include "SbBasicP.h"
#include "engines/SoSubEngineP.h"

#include <Inventor/SoDB.h>
#include <Inventor/errors/SoDebugError.h>
#include <Inventor/lists/SoEngineOutputList.h>

// *************************************************************************

SO_ENGINE_SOURCE(SoJumpVec3f);

// *************************************************************************

void
SoJumpVec3f::initClass(void)
{
  SO_ENGINE_INTERNAL_INIT_CLASS(SoJumpVec3f);
}

SoJumpVec3f::SoJumpVec3f() : SoJumpVec3f(SbVec3f(0.0f, 0.0f, 0.0f), SbVec3f(0.0f, 0.0f, -1.0f))
{
}

SoJumpVec3f::SoJumpVec3f(SbVec3fc& pos1, SbVec3fc& pos2) : SoJumpVec3f(pos1, pos2, SbVec3f(0.0f, -9.8f, 0.0f))
{
}

SoJumpVec3f::SoJumpVec3f(SbVec3fc& pos1, SbVec3fc& pos2, SbVec3fc& force)
{
  SO_ENGINE_INTERNAL_CONSTRUCTOR(SoJumpVec3f);

  SO_ENGINE_ADD_INPUT(in_time_, (SbTime::zero()));
  SO_ENGINE_ADD_INPUT(in.startPos, (pos1));
  SO_ENGINE_ADD_INPUT(in.landPos, (pos2));
  SO_ENGINE_ADD_INPUT(in.force, (force));
  SO_ENGINE_ADD_INPUT(in.isEnabled, (true));
  SO_ENGINE_ADD_INPUT(in.jump, ());

  SO_ENGINE_ADD_OUTPUT(out.currentPos, SoSFVec3f);
  SO_ENGINE_ADD_OUTPUT(out.isMoving, SoSFBool);

  this->in_time_.connectFrom(SoDB::getGlobalField("realTime"));

  this->out.currentPos.enable(FALSE);
  this->out.isMoving.enable(FALSE);

  SO_ENGINE_OUTPUT(out.currentPos,  SoSFVec3f, setValue(this->in.startPos.getValue()));
  SO_ENGINE_OUTPUT(out.isMoving, SoSFBool, setValue(false));
}

SoJumpVec3f::~SoJumpVec3f()
{
}

// *************************************************************************

// Documented in superclass. Overridden to not write connection to
// realTime global field.
void
SoJumpVec3f::writeInstance(SoOutput * out)
{
  // Note: the code in this method matches that of SoElapsedTime and
  // SoOneShot, so if any bugs are found and corrected, remember to
  // pass on the updates.

  // Disconnect from realTime field.
  SoField * connectfield = nullptr;
  SbBool connectfromrealTime =
    this->in_time_.getConnectedField(connectfield) &&
    connectfield == SoDB::getGlobalField("realTime");
  SbBool defaultflag = this->in_time_.isDefault();
  if (connectfromrealTime) {
    this->in_time_.disconnect();
    this->in_time_.setDefault(TRUE);
  }

  inherited::writeInstance(out);

  // Re-connect to realTime field.
  if (connectfromrealTime) {
    // Don't send notification when reconnecting to preserve the state
    // of the scenegraph between write passes.
    this->in_time_.connectFrom(connectfield, TRUE);
    this->in_time_.setDefault(defaultflag);
  }
}


void
SoJumpVec3f::inputChanged(SoField * which)
{
  if((which == &in_time_) && is_moving_) {
    // Notify listeners of new interpolation value
    this->out.currentPos.enable(true);
    this->out.isMoving.enable(true);
    return;
  }

  if((which == &in.jump) && in.isEnabled.getValue()) {
    // Initiate jump if enabled
    this->start_time_sec_ = this->in_time_.getValue().getValue();
    this->is_moving_ = true;
    this->out.currentPos.enable(true);
    this->out.isMoving.enable(true);
    return;
  }
}

// pos1 = pos0 + v0*t - 0.5*g*t*t
// pos1 = pos0 + v0 - 0.5*g
// (pos1 - pos0) + 0.5*g*t = v0

void
SoJumpVec3f::evaluate(void)
{
  float    t = static_cast<float>(in_time_.getValue().getValue() - start_time_sec_);
  SbVec3f  p;
  if (t > 1.0f) {
    p = in.landPos.getValue();
    is_moving_ = false;
  }
  else {
    SbVec3f p0 = in.startPos.getValue();
    SbVec3f p1 = in.landPos.getValue();
    SbVec3f  g = in.force.getValue();
    SbVec3f v0 = (p1 - p0) + 0.5f*g;
    p = v0 * t - 0.5f*g*t*t;
  }   
  this->out.currentPos.enable(true);
  this->out.isMoving.enable(true);

  SO_ENGINE_OUTPUT(out.currentPos, SoSFVec3f, setValue(p));
  SO_ENGINE_OUTPUT(out.isMoving, SoSFBool, setValue(is_moving_));

  this->out.isMoving.enable(false);
  this->out.currentPos.enable(false);
}
